// Sphere object

#include <math.h>
#include <iostream>

#include "../include/quadric.h"

#include "../my/include/Matrix.h"

// Plane defined as a vertex (in world space) as location
//                  and n which is the normal direction

Quadric::Quadric(double aa, double bb, double cc, double dd, double ee, double ff,
		double gg, double hh, double ii, double jj,Vertex &psp, float pr) {
	a = aa;
	b = bb;
	c = cc;
	d = dd;
	f = ff;
	g = gg;
	h = hh;
	i = ii;
	j = jj;
	sp = psp;
	r = pr;
	m.m[0][0] = a;	 	m.m[0][1] = b; 		m.m[0][2] = c; 		m.m[0][3] = d;
	m.m[1][0] = b; 		m.m[1][1] = e;	 	m.m[1][2] = f; 		m.m[1][3] = g;
	m.m[2][0] = c; 		m.m[2][1] = f; 		m.m[2][2] = h;	 	m.m[2][3] = i;
	m.m[3][0] = d; 		m.m[3][1] = g; 		m.m[3][2] = i; 		m.m[3][3] = j;


	typex = "Quadric";
	typei = 4;

}
bool Quadric::intersect(Ray &input_ray, Hit *hit) {
	Ray ray=input_ray;
	Vector ro;
	//transform
	if (transformed != 0) {
		ray.D.multiply(invers_matrix);
		ray.P.multiply(invers_matrix);
	}

	// equivalent to transforming ray into local sphere space

	ro.set(ray.P.x - sp.x, ray.P.y - sp.y, ray.P.z - sp.z);
	//http://www.bmsc.washington.edu/people/merritt/graphics/quadrics.html
//	Aqt2 + Bqt + Cq = 0 with
//	Aq = Axd2 + Byd2 + Czd2 + Dxdyd + Exdzd + Fydzd
//	Bq = 2*Axoxd + 2*Byoyd + 2*Czozd + D(xoyd + yoxd) + E(xozd + zoxd) + F(yozd + ydzo) + Gxd + Hyd + Izd
//	Cq = Axo2 + Byo2 + Czo2 + Dxoyo + Exozo + Fyozo + Gxo + Hyo + Izo + J
//
//	t0 =( - Bq - ((Bq2 - 4AqCq))^0.5)/ 2Aq
//	t1 =( - Bq + ((Bq2 - 4AqCq))^0.5)/ 2Aq

//	Check Aq = 0 (If Aq = 0 then t = -Cq / Bq)
//	If Aq <> 0, then check the discriminant. If (Bq2 - 4AqCq ) <0.0 then there is no intersection
//	Compute t0 and if t0 > 0 then done else compute t1
//	Once t is found compute Ri = (xi yi zi)
//	double Aq = a * ray.D.x * ray.D.x + b * ray.D.y * ray.D.y + c* ray.D.z * ray.D.z +
//				d * ray.D.x * ray.D.y + e * ray.D.x * ray.D.z + f * ray.D.y * ray.D.z;
//	double Bq = 2 * a * ray.P.x * ray.D.x + 2 * b * ray.P.y * ray.D.y +
//				2 * c * ray.P.z * ray.D.z + d * (ray.P.x * ray.D.y + ray.P.y * ray.D.x) +
//				e * (ray.P.x * ray.D.z + ray.P.z * ray.D.x) + f * (ray.P.y * ray.D.z + ray.P.z * ray.D.y) +
//				g * ray.D.x + h * ray.D.y + i * ray.D.z;
//	double Cq = a * ray.P.x * ray.P.x + b * ray.P.y * ray.P.y + c * ray.P.z * ray.P.z +
//			  	d * ray.P.x * ray.P.y + e * ray.P.x * ray.P.z + f * ray.P.y * ray.P.z +
//			  	g * ray.P.x + h * ray.P.y + i * ray.P.z + j;

	double Aq = a * ray.D.x * ray.D.x + b * ray.D.y * ray.D.y + c* ray.D.z * ray.D.z +
					d * ray.D.x * ray.D.y + e * ray.D.x * ray.D.z + f * ray.D.y * ray.D.z;
		double Bq = 2 * a * ro.x * ray.D.x + 2 * b * ro.y * ray.D.y +
					2 * c * ro.z * ray.D.z + d * (ro.x * ray.D.y + ro.y * ray.D.x) +
					e * (ro.x * ray.D.z + ro.z * ray.D.x) + f * (ro.y * ray.D.z + ro.z * ray.D.y) +
					g * ray.D.x + h * ray.D.y + i * ray.D.z;
		double Cq = a * ro.x * ro.x + b * ro.y * ro.y + c * ro.z * ro.z +
				  	d * ro.x * ro.y + e * ro.x * ro.z + f * ro.y * ro.z +
				  	g * ro.x + h * ro.y + i * ro.z + j;
	double t = 0.0;
	if(Aq == 0){
		t = -Cq / Bq;
	}else{
		double discriminant = Bq * Bq - 4 * Aq * Cq;
		if(discriminant < 0.0){
			//on intersection
			return false;
		}else{
			double t0 = (-Bq - sqrt(discriminant))/(2*Aq);
			if(t0 > 0){
				t = t0;
			}else{
				double t1 = (-Bq + sqrt(discriminant))/(2*Aq);
				t = t1;
			}
		}
	}

	double px = ray.P.x + t * ray.D.x;
	double py = ray.P.y + t * ray.D.y;
	double pz = ray.P.z + t * ray.D.z;


	hit->p.x = px;
	hit->p.y = py;
	hit->p.z = pz;

	hit->p.w = 1.0;



//	To compute Rn which is the normal at Ri take the partial derivatives of F with respect to x, y, z
//	Rn = [xn yn zn] = [dF/dx dF/dy dF/dz]
//
//	which gives
//	xn = 2*A*xi + D*yi + E*zi + G
//	yn = 2*B*yi + D*xi + F*zi + H
//	z n = 2*C*zi + E*xi + F*yi + I
	Vector ro1;
	ro1.set(px - sp.x, py - sp.y, pz - sp.z);

	if(r != 0.0){
		if(fabs(ro1.y) > fabs(r)){
			return false;
		}
		if(ro1.y * r < 0.0){
			return false;
		}
	}

	double px1 = ro1.x;
	double py1 = ro1.y;
	double pz1 = ro1.z;

	double nx = 2 * a * px1 + d * py1 + e * pz1 + g;
	double ny = 2 * b * py1 + d * px1 + f * pz1 + h;
	double nz = 2 * c * pz1 + e * px1 + f * py1 + i;

	hit->n.x = nx;
	hit->n.y = ny;
	hit->n.z = nz;
	hit->n.normalise();

	hit->t = t;
	hit->obj = this;

	hit->obj_mat = *this->obj_mat;

	return true;

}
